//
// Created by liz on 24-11-28.
//

#include "chassis.h"
#include "stdbool.h"
#include "settings.h"
#include <general_def.h>
#include <message.h>
#include <message_center.h>
#include "ins_task.h"

#include "dji_motor.h"
#include "user_lib.h"


#define WHEEL_BASE (174.2f)             //前后轮轴距
#define TRACK_WIDTH (234.0f)            //左右轮距
// #define RADIUS_WHEEL (47.8f)
#define RADIUS_WHEEL (50.5f)
#define M2006_REDUCTION (36.0f)

#define CENTER_GIMBAL_OFFSET_X (0)
#define CENTER_GIMBAL_OFFSET_Y (0)

/* 根据robot_def.h中的macro自动计算的参数 */
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)     // 半轴距
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)   // 半轮距
#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长

#define LINEAR_WHEEL_TO_RADIUS(v) (v/(RADIUS_WHEEL/1000.0))
#define LINEAR_WHEEL_TO_ANGLE(v) (LINEAR_WHEEL_TO_RADIUS(v)*RAD_2_DEGREE)            //v：m/s
#define ANGLE_WHEEL_TO_LINEAR(a) ((a*DEGREE_2_RAD)*(float)(RADIUS_WHEEL/1000.0))

attitude_t *ins;

static Publisher_t *chassis_pub;                   // 云台应用消息发布者(云台反馈给cmd)
static Subscriber_t *chassis_sub;                  // cmd控制消息订阅者

static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
static Chassis_Feedback_s chassis_feedback_send;

static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
static float chassis_vx, chassis_vy ,chassis_wz;     // 底盘速度m/s
static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,度/s
static float posoffset_x, posoffset_y;

static bool chassis_started = false;

Chassis_State_s chassis_state;          //底盘位置和速度里程计信息
float yaw_angle;                        //没有IMU时自己设置的yaw角度

PIDInstance pid_x,pid_y,pid_wz;


void Chassis_Init(void) {
    // 四个轮子的参数一样,改tx_id和反转标志位即可
    Motor_Init_Config_s chassis_motor_config = {
        .can_init_config.can_handle = &hcan1,
        .controller_param_init_config = {
            .speed_PID = {
                .Kp = 0.22, // 4.5
                .Ki = 0.006,  // 0
                .Kd = 0.0002,  // 0
                .IntegralLimit = 800,
                .Improve =  PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 2500,//6000
            },
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP,
            .close_loop_type = SPEED_LOOP ,
        },
        .motor_type = M2006,
    };
    //  @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
    chassis_motor_config.can_init_config.tx_id = 2;
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    motor_lf = DJIMotorInit(&chassis_motor_config);

    chassis_motor_config.can_init_config.tx_id = 1;
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    motor_rf = DJIMotorInit(&chassis_motor_config);

    chassis_motor_config.can_init_config.tx_id = 3;
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    motor_lb = DJIMotorInit(&chassis_motor_config);

    chassis_motor_config.can_init_config.tx_id = 4;
    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
    motor_rb = DJIMotorInit(&chassis_motor_config);

    PID_Init_Config_s pidx_init_config={
        .Kp = 5.0, // 4.5
        .Ki = 1.125,  // 0
        .Kd = 0.05,  // 0
        .DeadBand = 0,
        .IntegralLimit = 0.1,
        // .Improve =  PID_Integral_Limit | PID_Derivative_On_Measurement ,
        .Improve =  PID_Integral_Limit | PID_ChangingIntegrationRate ,
        .CoefA = 0.05,  //变速积分相关参数
        .CoefB = 0.02,
        .MaxOut = 0.4,
    };
    PIDInit(&pid_x,&pidx_init_config);
    PID_Init_Config_s pidy_init_config={
        .Kp = 7.0, // 4.5
        .Ki = 1.45,  // 0
        .Kd = 0.06,  // 0
        .DeadBand = 0,
        .IntegralLimit = 0.1,
        // .Improve =  PID_Integral_Limit | PID_Derivative_On_Measurement ,
        .Improve =  PID_Integral_Limit | PID_ChangingIntegrationRate ,
        .CoefA = 0.05,  //变速积分相关参数
        .CoefB = 0.01,
        .MaxOut = 0.4,
    };
    PIDInit(&pid_y,&pidy_init_config);
    PID_Init_Config_s yaw_pid_init_config={
        .Kp = 0.0165, // 4.5
        .Ki = 0.00120000029,  // 0
        .Kd = 0.000100000005,  // 0
        .DeadBand = 0.0,
        .IntegralLimit = 0.1,
        .Output_LPF_RC = 0.0005,
        .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_OutputFilter,
        .MaxOut = 0.25,
        // .MaxOut = 0.0,
    };
    PIDInit(&pid_wz,&yaw_pid_init_config);

    chassis_pub=PubRegister("chassis_feed",sizeof(Chassis_Feedback_s));
    chassis_sub=SubRegister("chassis_cmd",sizeof(Chassis_Ctrl_Cmd_s));

    ins=INS_Init();
}

#define LF_R (Sqrt(float_Square(HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X) + float_Square(HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y)))
#define RF_R (Sqrt(float_Square(HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X) + float_Square(HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y)))
#define LB_R (Sqrt(float_Square(HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X) + float_Square(HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y)))
#define RB_R (Sqrt(float_Square(HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X) + float_Square(HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y)))

#define LF_CENTER (LF_R * DEGREE_2_RAD)
#define RF_CENTER (RF_R * DEGREE_2_RAD)
#define LB_CENTER (LB_R * DEGREE_2_RAD)
#define RB_CENTER (RB_R * DEGREE_2_RAD)

/**
 * @brief 计算每个轮毂电机的输出,正运动学解算
 *        用宏进行预替换减小开销,运动解算具体过程参考教程
 */
static void MecanumCalculate()
{
    vt_lf = M2006_REDUCTION*((chassis_vx - chassis_vy - chassis_wz * LF_CENTER)/(RADIUS_WHEEL/1000.0f)*RAD_2_DEGREE);
    vt_rf = M2006_REDUCTION*((-chassis_vx - chassis_vy - chassis_wz * RF_CENTER)/(RADIUS_WHEEL/1000.0f)*RAD_2_DEGREE);
    vt_lb = M2006_REDUCTION*((chassis_vx + chassis_vy - chassis_wz * LB_CENTER)/(RADIUS_WHEEL/1000.0f)*RAD_2_DEGREE);
    vt_rb = M2006_REDUCTION*((-chassis_vx + chassis_vy - chassis_wz * RB_CENTER)/(RADIUS_WHEEL/1000.0f)*RAD_2_DEGREE);
}

static void ChassisOutput()
{
    DJIMotorSetRef(motor_lf, vt_lf);
    DJIMotorSetRef(motor_rf, vt_rf);
    DJIMotorSetRef(motor_lb, vt_lb);
    DJIMotorSetRef(motor_rb, vt_rb);
}

void Chassis_Feedback() {
    chassis_feedback_send.current_x=chassis_state.world_coordinate.position.x;
    chassis_feedback_send.current_y=chassis_state.world_coordinate.position.y;
    chassis_feedback_send.current_yaw=chassis_state.world_coordinate.position.yaw_total;
}

void update_chassis_state(Chassis_State_s *chassis_state, const float dt) {
    static int64_t last_ecd[4] = {0};           // 上一次的编码器值
    static int32_t wheel_ecd_diff[4];           // 四个轮子的编码器变化量
    static float wheel_angles[4];               // 四个轮子的角度
    static float wheel_distances[4];            // 每个轮子的位移（根据角度变化）

    // Step 1: 计算四个轮子的角度变化量
    wheel_ecd_diff[0] = motor_lf->measure.total_ecd - motor_lf->measure.ecd_offset - last_ecd[0];
    wheel_ecd_diff[1] = motor_rf->measure.total_ecd - motor_rf->measure.ecd_offset - last_ecd[1];
    wheel_ecd_diff[2] = motor_lb->measure.total_ecd - motor_lb->measure.ecd_offset - last_ecd[2];
    wheel_ecd_diff[3] = motor_rb->measure.total_ecd - motor_rb->measure.ecd_offset - last_ecd[3];

    // 更新上次编码器值
    last_ecd[0] = motor_lf->measure.total_ecd - motor_lf->measure.ecd_offset;
    last_ecd[1] = motor_rf->measure.total_ecd - motor_rf->measure.ecd_offset;
    last_ecd[2] = motor_lb->measure.total_ecd - motor_lb->measure.ecd_offset;
    last_ecd[3] = motor_rb->measure.total_ecd - motor_rb->measure.ecd_offset;


    // 计算每个轮子的角度变化（单位：度），编码器差值*转换系数/2006减速比
    wheel_angles[0] = wheel_ecd_diff[0]*ECD_ANGLE_COEF_DJI /M2006_REDUCTION;
    wheel_angles[1] = wheel_ecd_diff[1]*ECD_ANGLE_COEF_DJI /M2006_REDUCTION;
    wheel_angles[2] = wheel_ecd_diff[2]*ECD_ANGLE_COEF_DJI /M2006_REDUCTION;
    wheel_angles[3] = wheel_ecd_diff[3]*ECD_ANGLE_COEF_DJI /M2006_REDUCTION;

    // 计算每个轮子的位移（假设轮子的半径是r，可以根据角度和半径计算出轮子的位移）
    wheel_distances[0] = (float)wheel_angles[0] * DEGREE_2_RAD * RADIUS_WHEEL / 1000.0 ; // 每个轮子的位移（单位：米）
    wheel_distances[1] = (float)wheel_angles[1] * DEGREE_2_RAD * RADIUS_WHEEL / 1000.0 ;
    wheel_distances[2] = (float)wheel_angles[2] * DEGREE_2_RAD * RADIUS_WHEEL / 1000.0 ;
    wheel_distances[3] = (float)wheel_angles[3] * DEGREE_2_RAD * RADIUS_WHEEL / 1000.0 ;


    // 更新轮子位移到状态结构
    chassis_state->chassis_coordinate.wheel_speeds.V_FL = wheel_distances[0]/dt;
    chassis_state->chassis_coordinate.wheel_speeds.V_FR = wheel_distances[1]/dt;
    chassis_state->chassis_coordinate.wheel_speeds.V_RL = wheel_distances[2]/dt;
    chassis_state->chassis_coordinate.wheel_speeds.V_RR = wheel_distances[3]/dt;

    // Step 2: 根据轮子位移计算车体的总位移
    // 假设车体是一个麦克纳姆轮机器人，四个轮子的位移可以计算车体的位移
    // 可以考虑每个轮子的对车体的贡献。此处简化为平均值

    static float delta_x,delta_y,delta_yaw,yaw,last_yaw;

    if(chassis_started)
    {
        delta_x = 0.25f * (wheel_distances[0] - wheel_distances[1] + wheel_distances[2] - wheel_distances[3]);
        delta_y = 0.25f * (-wheel_distances[0] - wheel_distances[1] + wheel_distances[2] + wheel_distances[3]);
    }
    else
    {
        delta_x = 0;
        delta_y = 0;
    }


#ifdef IMU_FEEDBACK
    last_yaw = yaw;
    yaw = ins->yaw;
    delta_yaw = yaw - last_yaw;

    chassis_state->world_coordinate.position.yaw_angle = yaw;
    chassis_state->world_coordinate.position.yaw_total = ins->yawTotalAngle;
#else
    delta_yaw = 0.25f * (-wheel_distances[0] / LF_R - wheel_distances[1] / RF_R - wheel_distances[2] / LB_R - wheel_distances[3] / RB_R) *1000.0f * RAD_2_DEGREE;
    // 更新车体位置
    yaw = chassis_state->world_coordinate.position.yaw_angle; // 当前车体的偏航角
    chassis_state->world_coordinate.position.yaw_total += delta_yaw;
#endif

    static float cosyaw,sinyaw;
    cosyaw = cosf(yaw*DEGREE_2_RAD);
    sinyaw = sinf(yaw*DEGREE_2_RAD);

    // 更新车体坐标系下的位置（如果需要）
    chassis_state->chassis_coordinate.position.x += delta_x;
    chassis_state->chassis_coordinate.position.y += delta_y;

    // Step 3: 通过轮编码器数据更新车体位置和姿态
    chassis_state->world_coordinate.position.x += cosyaw * delta_x - sinyaw * delta_y;
    chassis_state->world_coordinate.position.y += sinyaw * delta_x + cosyaw * delta_y;

    // chassis_state->chassis_coordinate.position.yaw += delta_yaw;
}




static void Odometer(void)
{
    static uint32_t DWT_CNT;
    static float dt;
    dt = DWT_GetDeltaT(&DWT_CNT);

    update_chassis_state(&chassis_state,dt);
}

static void coordinate_closeloop(void)
{
    static float sin_theta, cos_theta;

    pid_x.MaxOut=chassis_cmd_recv.max_speed;
    pid_y.MaxOut=chassis_cmd_recv.max_speed;

    //计算夹角的sin和cos值
    cos_theta = arm_cos_f32(ins->yaw * DEGREE_2_RAD);
    sin_theta = arm_sin_f32(ins->yaw * DEGREE_2_RAD);

    float chassis_target_x = chassis_cmd_recv.target_x + chassis_cmd_recv.offset_x;
    float chassis_target_y = chassis_cmd_recv.target_y + chassis_cmd_recv.offset_y;

    //位移闭环
    PIDCalculate(&pid_x,chassis_state.world_coordinate.position.x,chassis_target_x);
    PIDCalculate(&pid_y,chassis_state.world_coordinate.position.y,chassis_target_y);
    PIDCalculate(&pid_wz,chassis_state.world_coordinate.position.yaw_total,chassis_cmd_recv.target_yaw);

    chassis_vx = pid_x.Output * cos_theta + pid_y.Output * sin_theta;
    chassis_vy = -pid_x.Output * sin_theta + pid_y.Output * cos_theta;
    chassis_wz=pid_wz.Output;
}


//float target_x,target_y,target_wz;

void Chassis_Task() {
    SubGetMessage(chassis_sub, &chassis_cmd_recv);
    //里程计计算，相当于逆运动学解算
    Odometer();

    coordinate_closeloop();

    //麦轮控制
    MecanumCalculate();
    ChassisOutput();
    Chassis_Feedback();
    PubPushMessage(chassis_pub,&chassis_feedback_send);
}


void Chassis_Start()
{
    chassis_started=true;
}